﻿#include "Multivector.h"


using namespace std;
using namespace mygalib;


//Empty default constructor
Multivector::Multivector()
{

}

//Calculate the sign using the canonical_reordering
double mygalib::calc_sign (unsigned int bitmap1, unsigned int bitmap2)
{
	return canonical_reordering(bitmap1,bitmap2);
}

//Regressive Product
Multivector Multivector::rp(Multivector& b,int n)
{
	return mygalib::rp(*this,b,n);
}

//Regressive Product
Multivector Multivector::rp(Multivector& b,Multivector& I)
{
	return mygalib::rp(*this,b,I);
}

Multivector Multivector::op(Multivector& b)
{
	return mygalib::op(*this,b);
}

//Geometric Product
Multivector Multivector::gp(Multivector& b)
{
	return mygalib::gp(*this,b);
}

//Geometric Product
Multivector Multivector::operator*(Multivector& b)
{
	return mygalib::gp(*this,b);
}

//Left Contraction
Multivector Multivector::lc(Multivector& b)
{
	return mygalib::lc(*this,b);
}

//Left Contration
Multivector Multivector::operator<<(Multivector& b)
{
	return mygalib::lc(*this,b);
}

//Right Contraction
Multivector Multivector::rc(Multivector& b)
{
	return mygalib::rc(*this,b);
}

//Right Contraction
Multivector Multivector::operator>>(Multivector& b)
{
	return mygalib::rc(*this,b);
}

//Scalar
double Multivector::sc(Multivector& b)
{
	return mygalib::sc(*this,b);
}

//Delta Product
Multivector Multivector::dp(Multivector& b)
{
	return mygalib::dp(*this,b);
}

//Addition
Multivector Multivector::add(Multivector& b)
{
	return mygalib::add(*this,b);
}

//Subtraction
Multivector Multivector::sub(Multivector& b)
{
	return mygalib::sub(*this,b);
}

//Multiplication
Multivector Multivector::mul(double b)
{
	return mygalib::mul(*this,b);
}

Multivector Multivector::div(double b)
{
	return mygalib::div(*this,b);
}


Multivector Multivector::operator/(double b)
{
	return mul(1.0/b);
}

//Identity
Multivector mygalib::I(int dim)
{
	Multivector r = Multivector();
	unsigned int a = 1;
	for (int i = 0; i < (dim-1); i++)
	{
		a = a << 1;
		a = a | 1;
	}
	r(a) = 1;
	return r;
}

//Pow
int mygalib::pot(int i,int j)
{
	int result = 1;
	if (j == 0)
	{
		return 1;
	}

	do
	{
		result *= i;
		j--;
	}
	while(j > 0);

	return result;
}

//Inverse
Multivector Multivector::inv()
{
	Multivector c = rev();
	map<unsigned int, double>::iterator iter;
	double sn = squared_norm();

	for (iter = c.coeff.begin(); iter != c.coeff.end(); ++iter)
	{
		c.coeff[iter->first] = iter->second /sn;
	}

	return c;
	
}

//Get the coefficient
double& Multivector::operator () (unsigned int i) {
    return coeff[i];
}

//Get the coefficient
double& Multivector::operator () (string i) {
	return coeff[bin_str_to_int(i)];
}

//Get the coefficient
const double& Multivector::operator () (string i) const {
	return coeff.at(bin_str_to_int(i));
}

//Atribute the coefficient
const double& Multivector::operator () (unsigned int i) const {
	return coeff.at(i);
}

Multivector Multivector::rev()
{
	Multivector r = Multivector();
	map<unsigned int, double>::iterator iter;
	for (iter = coeff.begin(); iter != coeff.end(); ++iter)
	{
		int k = bit_count(iter->first);
		double sign = pot(-1, (k*(k-1)) / 2);
		r.coeff[iter->first] = sign*iter->second;
	}
	return r;
}

//Constructor
Multivector::Multivector(unsigned int i)
{
	coeff[i] = 1;
}

//Constructor
Multivector::Multivector(string basis)
{
	coeff[bin_str_to_int(basis)] = 1;
}

//Squared Normalization
double Multivector::squared_norm()
{
	Multivector a = rev();
	return sc(a);
}

//Multiplication between scalar and a multivector
Multivector mygalib::operator*(double s, Multivector& v)
{
	return v * s;
}

Multivector Multivector::operator-(Multivector& b)
{
	return sub(b);
}

Multivector Multivector::operator*(double b)
{
	return mul(b);
}

Multivector Multivector::operator+(Multivector& b)
{
	return add(b);
}

//Take the highest grade
////////(Forgive coefficients with 0?????)
int Multivector::grade()
{
	map<unsigned int, double>::iterator iterA;
	int g = 0;
	int temp = 0;
	for (iterA = coeff.begin(); iterA != coeff.end(); ++iterA)
	{
		temp = bit_count(iterA->first);
		if (temp > g && (iterA->second != 0))
		{
			g = temp;
		}
	}
	return g;
}

double mygalib::canonical_reordering(unsigned int bitmap1,unsigned int  bitmap2)
{
	// Count the number of basis vectors swaps
	unsigned int sum = 0;

	bitmap1 = bitmap1 >> 1;

	while (bitmap1 != 0)
	{
		sum = sum + bit_count(bitmap1 & bitmap2);
		bitmap1 = bitmap1 >> 1;
	}
	return ((sum & 1) == 0) ? 1.0 : -1.0;
}

unsigned int mygalib::bit_count(unsigned int n)
{
    int count = 0;
    while (n)
    {
        count++;
        n &= (n - 1);     
    }
    return count ;
}

Multivector Multivector::operator^(Multivector& b)
{
	return op(b);
}

//Rotor
Multivector Multivector::rotor(double angle)
{
	return mygalib::rotor(*this,angle);
}

void Multivector::print()
{
	map<unsigned int, double>::iterator iter;
	bool flag1 = false;
	bool flag2 = false;
	for (iter = coeff.begin(); iter != coeff.end(); ++iter)
	{
		if (iter->second != 0)
		{
			flag2 = true;
			if (flag1)
			{
				cout << "  +  "  << iter->second << " "<< bitmap_to_string(iter->first);
			}
			else
			{
				cout << iter->second << " "<< bitmap_to_string(iter->first);
				flag1 = true;
			}
		}
	}
	if (!flag2)
	{
		cout << "0";
	}
	cout << endl;
}

void Multivector::print_debug()
{
	map<unsigned int, double>::iterator iter;
	for (iter = coeff.begin(); iter != coeff.end(); ++iter)
	{
		cout << iter->first << " "<< iter->second << "\t";
	}
	cout << endl;
}

string Multivector::bitmap_to_string(unsigned int bitmap)
{
	string a;
	bool flag = false;
	int i = 1;
	while (bitmap != 0)
	{
		if ( (1 & bitmap) != 0) 
		{
			if (flag)
			{
				a = a + "^e" + to_string((long long)i);
			}
			else
			{
				a = "e" + to_string((long long)i);
				flag = true;
			}
		}
		bitmap = bitmap >> 1;
		i++;
	}
	return a;
}

//Check if it is a blade
bool Multivector::is_blade()
{
	Multivector e1 = Multivector("1");
	Multivector M_hat = grade_involution();
	Multivector M_inv = inv();
	Multivector M_rev = rev();
	if (M_hat.gp(M_inv).grade() == 0 &&
		M_hat.gp(M_inv) == M_inv.gp(M_hat) &&
		M_hat.gp(e1).gp(M_rev).grade() == 1)
	{
		return true;
	}
	return false;
}

//Grade Involution
Multivector Multivector::grade_involution()
{
	return pot(-1,this->grade()) * (*this);
}

vector<Multivector> mygalib::_orthogonal_basis(vector<Multivector> b)
{
	Multivector S = Multivector();
	vector<Multivector> n;
	S(0) = 1;
	vector<Multivector>::iterator iter;
	for (iter = b.begin(); iter != b.end(); ++iter)
	{
		Multivector T = S ^ (*iter);
		if (T.squared_norm() == 0)
		{
			return n;
		}
		n.push_back(S.inv().lc(T));
		S = T;
	}
	return n;
}

Multivector Multivector::dual(int dim)
{
	return lc(I(dim).inv());
}

vector<Multivector> mygalib::_blade_factorization(Multivector& B)
{
	double beta = sqrt(B.squared_norm());
	Multivector C = B*(1/beta);

	vector<Multivector> fs;
	Multivector f = Multivector();

	map<unsigned int, double>::iterator iter;

	for (iter = B.it_begin(); iter != B.it_end(); ++iter)
	{
		Multivector e = Multivector();
		e(iter->first) = iter->second;

		Multivector p = (e.lc(C.inv())).lc(C);
		f = p*(1/sqrt(p.squared_norm()));
		fs.push_back(f);
		C = f.inv().lc(C);
	}
	f = C*(1/sqrt(C.squared_norm()));
	fs.push_back(f);
	return fs;
}

vector<Multivector> Multivector::_meet_join (Multivector A, Multivector B, int dim)
{
	vector<Multivector> res;
	int a_grade = A.grade();
	int b_grade = B.grade();

	Multivector M = Multivector();
	M(0) = 1;
	Multivector J = Multivector();
	J = I(dim);

	if (a_grade > b_grade)
	{
		int sign = pot(-1, (dim-a_grade) * (dim-b_grade)); 
		Multivector temp = B;
		B = sign * A;
		A = sign * temp;
	}

	Multivector S = A.dp(B).dual(dim);
	vector<Multivector> Ss = _blade_factorization(S);
	vector<Multivector>::iterator iterS;
	for (iterS = Ss.begin(); iterS != Ss.end(); ++iterS)
	{
		Multivector p = ( (*iterS).lc(A.inv()).lc(A));
		Multivector r = ( (*iterS) ^ A).rc(A.inv());

		if (p.grade() != 0 && p(0) != 0)
		{
			M = M ^ p;
		}
		if (r.grade() != 0 && r(0) != 0)
		{
			J = r.lc(J);
		}
	}

	res.push_back(M);
	res.push_back(J);

	return res;

}

//Return the begin of the iterator of the hash map
map<unsigned int, double>::iterator Multivector::it_begin()
{
	return coeff.begin();
}

//Return the end of the iterator of the hash map
map<unsigned int, double>::iterator Multivector::it_end()
{
	return coeff.end();
}

Multivector Multivector::_join(Multivector& b, int dim)
{
	return _meet_join((*this),b,dim)[0];
}

Multivector Multivector::join(Multivector& b, int dim)
{
	return mygalib::join(*this,b,dim);
}

Multivector Multivector::_meet(Multivector& b, int dim)
{
	return _meet_join((*this),b,dim)[1];
}

Multivector mygalib::rotor(Multivector& M, double angle)
{
    double norm = sqrt(M.squared_norm());
    Multivector p = M / norm;
    double half_angle = angle / 2.0;
    p = p * (-sin(half_angle));
	p(0) += cos(half_angle);
 
	return p;
}

//Return the Dual of a multivector, passing the dimension as a parameter
Multivector Multivector::undual(int dim)
{
	return lc(I(dim));
}

//Check if 2 multivectors are equal
bool Multivector::equals(Multivector& b)
{
	return mygalib::equals(*this,b);
}

//Check if 2 multivectors are equal
//bool Multivector::operator==(Multivector& b)
//{
//	return mygalib::equals(*this,b);
//}

//Return a Blade with the correspoding grade
Multivector Multivector::extract_grade(int grade)
{
	Multivector d = Multivector();
	unsigned int bitmap;
	unsigned int currGrade;
	map<unsigned int, double>::iterator iter;
	for (iter = it_begin(); iter != it_end(); ++iter)
	{
		bitmap = iter->first;
		currGrade = bit_count(bitmap);
		if (grade == currGrade)
		{
			d.coeff[bitmap] = iter->second;
		}
	}
	return d;
}

//Binary String to unsigned int
unsigned int mygalib::bin_str_to_int(string s)
{
	unsigned int res = 0;
	int size = s.size();
	for (int i = 0; i < size; i++)
	{
		if (s[i] == '1')
		{
			res += pot(2,size-i-1);
		}
	}
	return res;
}

//Return the Meet between two multivectors in a determined dimension
Multivector Multivector::meet(Multivector& b, int dim)
{
	return mygalib::meet(*this,b,dim);
}

////Return the Meet between two multivectors in a determined dimension
Multivector mygalib::meet(Multivector& a, Multivector& b, int dim)
{
	return (b.dual(dim) ^ a.dual(dim)).undual(dim);
}
//
////Return the Join between two multivectors in a determined dimension
Multivector mygalib::join(Multivector& a, Multivector& b, int dim)
{
	return (a >> meet(a,b,dim).inv() ) ^ b;
}

//////////Return the Delta Product between 2 multivectors
Multivector mygalib::dp(Multivector& a, Multivector& b)
{
	Multivector c = gp(a,b);
	int g = c.grade();
	return c.extract_grade(g);
}

///////Return the Outer Product between 2 multivectors
Multivector mygalib::op(Multivector& a, Multivector& b)
{
	Multivector c = Multivector();
	map<unsigned int, double>::iterator iterA;
	map<unsigned int, double>::iterator iterB;

	unsigned int bitmap1;
	unsigned int bitmap2;
	unsigned int bitmap;

	for (iterA = a.it_begin(); iterA != a.it_end(); ++iterA)
	{
		for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
		{
			if ( (iterA->first & iterB->first) != 0)
			{
				c(0) = 0;
			}
			else
			{
				bitmap1 = iterA->first;
				bitmap2 = iterB->first;
				bitmap = bitmap1 | bitmap2;

				c(bitmap) = c(bitmap) + calc_sign(bitmap1, bitmap2) * iterA->second * iterB->second;
			}
		}
	}
	return c;
}

////////Return the addition of 2 multivectors
Multivector mygalib::add(Multivector& a, Multivector& b)
{
	Multivector c = a;
	map<unsigned int, double>::iterator iterB;

	for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
	{
		c(iterB->first) += iterB->second;
	}
	return c;
}

/////////Return the subtraction of 2 multivectors
Multivector mygalib::sub(Multivector& a, Multivector& b)
{
	Multivector c = a;
	map<unsigned int, double>::iterator iterB;

	for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
	{
		c(iterB->first) -= iterB->second;
	}

	return c;
}
//////////Return the multiplication of 2 multivectors
Multivector mygalib::mul(Multivector& a, double b)
{
	Multivector c = Multivector();
	map<unsigned int, double>::iterator iter;

	for (iter = a.it_begin(); iter != a.it_end(); ++iter)
	{
		c(iter->first) = b*iter->second;
	}

	return c;
}
/////////////Return the division of a multivectors and a scalar
Multivector mygalib::div(Multivector& a, double b)
{
	return mul(a,1.0/b);
}

Multivector mygalib::rp(Multivector& a, Multivector& b, Multivector& I)
{
	return ((b<<I.inv())^(a<<I.inv()))<<I;
}

//////////////Return the regressive product of a multivectors and a scalar
Multivector mygalib::rp(Multivector& a, Multivector& b, int n)
{
	Multivector c = Multivector();
	map<unsigned int, double>::iterator iterA;
	map<unsigned int, double>::iterator iterB;

	unsigned int bitmap1;
	unsigned int bitmap2;
	unsigned int bitmap;
	//unsigned int n;

	for (iterA = a.it_begin(); iterA != a.it_end(); ++iterA)
	{
		for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
		{
			bitmap1 = iterA->first;
			bitmap2 = iterB->first;

			bitmap = bitmap1 & bitmap2;

			//n = bit_count(bitmap1 | bitmap2);

			// Check whether the dual of the two basis blades are independent
			if (bit_count(bitmap1) + bit_count(bitmap2) - bit_count(bitmap) != n )
			{
				c(0) = 0;
			}
			else 
			{
				c(bitmap) = c(bitmap) + calc_sign(bitmap1^bitmap2, bitmap2^bitmap1) * iterA->second * iterB->second;
			}
		}
	}
	return c;
}

///////////Return the Geometric Product between 2 multivectors
Multivector mygalib::gp(Multivector& a, Multivector& b)
{
	Multivector c = Multivector();
	map<unsigned int, double>::iterator iterA;
	map<unsigned int, double>::iterator iterB;

	unsigned int bitmap1;
	unsigned int bitmap2;
	unsigned int bitmap;

	for (iterA = a.it_begin(); iterA != a.it_end(); ++iterA)
	{
		for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
		{
			bitmap1 = iterA->first;
			bitmap2 = iterB->first;
			bitmap = bitmap1 ^ bitmap2;

			c(bitmap) = c(bitmap) + calc_sign(bitmap1, bitmap2) * iterA->second * iterB->second;
		}
	}
	return c;
}

/////////Return the Left Contration between 2 multivectors
Multivector mygalib::lc(Multivector& a, Multivector& b)
{
	//C = A B
	//Calculate the Geometric Product
	Multivector c = gp(a,b);
	//Take the highest grade from A
	int gradeA = a.grade();
	//Take the highest grade from A
	int gradeB = b.grade();
	//The expected grade
	int g = gradeB - gradeA;
	return c.extract_grade(g);
}

//////////Return the Right Contration between 2 multivectors
Multivector mygalib::rc(Multivector& a, Multivector& b)
{
	Multivector c = gp(a,b);
	int gradeA = a.grade();
	int gradeB = b.grade();
	int g = gradeA - gradeB;
	return c.extract_grade(g);
}

/////Return the Scalar between 2 multivectors
double mygalib::sc(Multivector& a, Multivector& b)
{
	Multivector c = a.gp(b);
	return c(0);
}

//Check if 2 multivectors are equal
bool mygalib::operator==(Multivector& a, Multivector& b)
{
	return equals(a,b);
}

//Check if 2 multivectors are equal
bool mygalib::equals(Multivector& a, Multivector& b)
{
	map<unsigned int, double>::iterator iterA;
	map<unsigned int, double>::iterator iterB;

	for (iterA = a.it_begin(); iterA != a.it_end(); ++iterA)
	{
		if (a(iterA->first) != b(iterA->first))
		{
			return false;
		}
	}

	for (iterB = b.it_begin(); iterB != b.it_end(); ++iterB)
	{
		if (a(iterA->first) != b(iterA->first))
		{
			return false;
		}
	}
	return true;
}

string mygalib::bitmap_to_bin_str(unsigned int bitmap)
{
	string a;
	bool flag = false;
	while (bitmap != 0)
	{
		if ( (1 & bitmap) != 0) 
		{
			a = a + "1";
		}
		else
		{
			a = a + "0";
		}
		bitmap = bitmap >> 1;
	}

	return string ( a.rbegin(), a.rend() );
}

Multivector mygalib::ggp(Multivector& a, Multivector& b, mat space)
{

	//M = Q S Q'

	//Q is an orthonormal matrix whose ith column is the eigenvector qi of M
	//S is a diagonal matrix whose ith diagonal element is the eigenvalue λi of M

	//1. Compute the eigenvectors and eigenvalues of the metric matrix
	//2. Represent the input with respect to the eigenbasis
	//   - Apply a change of basis using the inverse (i.e., the transpose) of
	//   the eigenvector matrix

	//fi = Q'ei

	//3. Compute the geometric product on this new orthogonal basis
	//   - The eigenvalues specify the new orthogonal metric

	//4. Get back to the original basis
	//   - Apply a change of basis using the original eigenvector matrix

	//ei = Q fi

	mat U;
	vec s;
	mat V;

	svd(U,s,V, space,"standard"); 

	mat Q_t = diagmat(s).i() * U.t();
	mat Q = U * diagmat(s);
	Multivector aa = basis_convertion(a,Q_t);
	Multivector bb = basis_convertion(b,Q_t);
	Multivector r_temp = gp(aa,bb);

	return basis_convertion(r_temp,Q);

}

//Basis convertion
Multivector mygalib::basis_convertion(Multivector& a, mat Q)
{
	Q.print("Q");
	Multivector r = Multivector();
	map<unsigned int, double>::iterator iter;
	for (iter = a.it_begin(); iter != a.it_end(); ++iter)
	{
		double coeff = 1;
		unsigned int bitmap = iter->first;
		int i = 0;
		while (bitmap != 0)
		{
			if ( (1 & bitmap) != 0) 
			{
				coeff = coeff * Q(i,i);
			}
			bitmap = bitmap >> 1;
			i++;
		}
		r(iter->first) = iter->second * coeff;
	}

	return r;
}

//Multivector Geometric Product
//Multivector Multivector::ggp(Multivector& b)
//{
//	return mygalib::ggp(*this,b,Metric::metric);
//}

////Set Metric
//void Multivector::set_metric(mat m)
//{
//	mygalib::Metric::metric = m;
//}
//
////Get Metric
//mat Multivector::get_metric()
//{
//	return mygalib::Metric::metric;
//}

//exp
Multivector mygalib::ex(Multivector& a)
{
	Multivector r = Multivector();
	map<unsigned int, double>::iterator iter;
	for (iter = a.it_begin(); iter != a.it_end(); ++iter)
	{
		r(iter->first) = exp(iter->second);
	}
	return r;
}

//sqrt
Multivector mygalib::sqr(Multivector& a)
{
	Multivector r = Multivector();
	map<unsigned int, double>::iterator iter;
	for (iter = a.it_begin(); iter != a.it_end(); ++iter)
	{
		r(iter->first) = sqrt(iter->second);
	}
	return r;
}

//pow
Multivector mygalib::po(Multivector& a, double b)
{
	Multivector r = Multivector();
	map<unsigned int, double>::iterator iter;
	for (iter = a.it_begin(); iter != a.it_end(); ++iter)
	{
		r(iter->first) = pow(iter->second,b);
	}
	return r;
}

//Log
//Multivector mygalib::lo(Multivector& a)
//{
//	return a.extract_grade(2)/ sqr(a.extract_grade(2).squared_norm()) * atan2( sqr(a.extract_grade(2)) / sqr(a.extract_grade(0)));
//}